﻿using System;


namespace UltrasonicTDPlatform
{
    /// <summary>
    /// 范数
    /// </summary>
    public class Norm
    {
        /// <summary>
        /// 返回向量v的1-范数
        /// </summary>
        /// <param name="v"></param>
        /// <returns></returns>
        public static double One(Vector v)
        {
            double sum = 0;
            for (int i = 0; i < v.Length; i++)
            {
                sum += Math.Abs(v[i]);
            }
            return sum;
        }

        /// <summary>
        /// 返回向量v的2-范数
        /// </summary>
        /// <param name="v"></param>
        /// <returns></returns>
        public static double Two(Vector v)
        {
            double sum = 0;
            for (int i = 0; i < v.Length; i++)
            {
                sum += v[i] * v[i];
            }
            return Math.Sqrt(sum);
        }

        /// <summary>
        /// 返回向量v的∞-范数
        /// </summary>
        /// <param name="v"></param>
        /// <returns></returns>
        public static double Infinity(Vector v)
        {
            double max = 0;
            for (int i = 0; i < v.Length; i++)
            {
                if (max < Math.Abs(v[i]))
                    max = Math.Abs(v[i]);
            }
            return max;
        }

        /// <summary>
        /// 返回矩阵m的1-范数
        /// </summary>
        /// <param name="m"></param>
        /// <returns></returns>
        public static double One(Matrix m)
        {
            double max = 0;
            for (int i = 0; i < m.ColumnCount; i++)
            {
                double x = Norm.One(m.GetColumn(i));
                if (x > max)
                    max = x;
            }
            return max;
        }

        /// <summary>
        /// 返回矩阵m的∞-范数
        /// </summary>
        /// <param name="m"></param>
        /// <returns></returns>
        public static double Infinity(Matrix m)
        {
            double max = 0;
            for (int i = 0; i < m.RowCount; i++)
            {
                double x = Norm.One(m.GetRow(i));
                if (x > max)
                    max = x;
            }
            return max;
        }

        /// <summary>
        /// 返回矩阵的Frobenius范数
        /// </summary>
        /// <param name="m"></param>
        /// <returns></returns>
        public static double Frobenius(Matrix m)
        {
            double sum = 0;
            for (int i = 0; i < m.RowCount; i++)
            {
                for (int j = 0; j < m.ColumnCount; j++)
                {
                    sum += m[i, j] * m[i, j];
                }
            }
            return Math.Sqrt(sum);
        }
    }
    /// <summary>
    /// 向量
    /// </summary>
    public class Vector
    {
        #region 保护成员
        /// <summary>
        /// 存储向量数据的数组
        /// </summary>
        protected double[] _data;
        #endregion

        #region 公有属性
        /// <summary>
        /// 向量元素的个数
        /// </summary>
        public int Length => _data.Length;
        #endregion

        #region 构造方法
        /// <summary>
        /// 构造一个空向量
        /// </summary>
        public Vector()
        {
            _data = new double[0];
        }

        /// <summary>
        /// 构造一个元素个数为length的向量，默认元素为0；
        /// 当fillOne为true时，元素均为1
        /// </summary>
        /// <param name="length"></param>
        /// <param name="fillOne"></param>
        public Vector(int length, bool fillOne = false)
        {
            _data = new double[length];
            if (fillOne)
            {
                for (int i = 0; i < length; i++)
                {
                    _data[i] = 1;
                }
            }
        }

        /// <summary>
        /// 根据一系列元素构造向量
        /// </summary>
        /// <param name="values"></param>
        public Vector(params double[] values)
        {
            _data = new double[values.Length];
            Array.Copy(values, _data, values.Length);
        }
        #endregion

        #region 公共方法
        /// <summary>
        /// 访问或设置向量中索引为index的元素
        /// </summary>
        /// <param name="index"></param>
        /// <returns></returns>
        public double this[int index]
        {
            get => _data[index];
            set => _data[index] = value;
        }

        /// <summary>
        /// 正号，返回v1的拷贝
        /// </summary>
        /// <param name="v1"></param>
        /// <returns></returns>
        public static Vector operator +(Vector v1)
        {
            return v1.Copy();
        }

        /// <summary>
        /// 负号，返回v1的相反向量
        /// </summary>
        /// <param name="v1"></param>
        /// <returns></returns>
        public static Vector operator -(Vector v1)
        {
            Vector v = new Vector(v1.Length);
            for (int i = 0; i < v.Length; i++)
            {
                v[i] = -v1[i];
            }
            return v;
        }

        /// <summary>
        /// 向量相加
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Vector operator +(Vector v1, Vector v2)
        {
            if (v1.Length != v2.Length)
                throw new Exception("两向量元素个数不同，不能相加！");
            Vector v = new Vector(v1.Length);
            for (int i = 0; i < v.Length; i++)
            {
                v[i] = v1[i] + v2[i];
            }
            return v;
        }

        /// <summary>
        /// 向量相减
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Vector operator -(Vector v1, Vector v2)
        {
            if (v1.Length != v2.Length)
                throw new Exception("两向量元素个数不同，不能相减！");
            Vector v = new Vector(v1.Length);
            for (int i = 0; i < v.Length; i++)
            {
                v[i] = v1[i] - v2[i];
            }
            return v;
        }

        /// <summary>
        /// 向量点乘
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static double operator *(Vector v1, Vector v2)
        {
            if (v1.Length != v2.Length)
                throw new Exception("两向量元素个数不同，不能点乘！");
            double sum = 0;
            for (int i = 0; i < v1.Length; i++)
            {
                sum += v1[i] * v2[i];
            }
            return sum;
        }

        /// <summary>
        /// 数乘向量
        /// </summary>
        /// <param name="scale"></param>
        /// <param name="v1"></param>
        /// <returns></returns>
        public static Vector operator *(double scale, Vector v1)
        {
            Vector v = new Vector(v1.Length);
            for (int i = 0; i < v1.Length; i++)
            {
                v[i] = v1[i] * scale;
            }
            return v;
        }

        /// <summary>
        /// 数乘向量
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="scale"></param>
        /// <returns></returns>
        public static Vector operator *(Vector v1, double scale)
        {
            return scale * v1;
        }

        /// <summary>
        /// 向量除数
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="scale"></param>
        /// <returns></returns>
        public static Vector operator /(Vector v1, double scale)
        {
            return 1 / scale * v1;
        }

        /// <summary>
        /// 按元素相乘
        /// </summary>
        /// <param name="v1"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public Vector Mutiply(Vector v1)
        {
            if (Length != v1.Length)
                throw new Exception("两向量元素个数不同，不能按元素相乘！");
            Vector v = new Vector(v1.Length);
            for (int i = 0; i < v.Length; i++)
            {
                v[i] = this[i] * v1[i];
            }
            return v;
        }

        /// <summary>
        /// 按元素相除
        /// </summary>
        /// <param name="v1"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public Vector Divide(Vector v1)
        {
            if (Length != v1.Length)
                throw new Exception("两向量元素个数不同，不能按元素相除！");
            Vector v = new Vector(v1.Length);
            for (int i = 0; i < v.Length; i++)
            {
                v[i] = this[i] / v1[i];
            }
            return v;
        }

        /// <summary>
        /// 是否为零向量
        /// </summary>
        /// <returns></returns>
        public bool IsZero()
        {
            for (int i = 0; i < Length; i++)
            {
                if (this[i] != 0)
                    return false;
            }
            return true;
        }

        /// <summary>
        /// 向量是否相等
        /// </summary>
        /// <param name="v1"></param>
        /// <returns></returns>
        public bool Equals(Vector v1)
        {
            if (Length != v1.Length)
                return false;
            for (int i = 0; i < Length; i++)
            {
                if (this[i] != v1[i])
                    return false;
            }
            return true;
        }

        /// <summary>
        /// 返回元素个数为end-begin的子向量，包括索引为begin到end-1的所有元素
        /// </summary>
        /// <param name="begin"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        public Vector SubVector(int begin, int end)
        {
            Vector v = new Vector(end - begin);
            Array.Copy(_data, begin, v._data, 0, end - begin);
            return v;
        }

        /// <summary>
        /// 拼接向量，当前向量在前，v1在后
        /// </summary>
        /// <param name="v1"></param>
        /// <returns></returns>
        public Vector Concat(Vector v1)
        {
            Vector v = new Vector(Length + v1.Length);
            CopyTo(v);
            v1.CopyTo(v, Length);
            return v;
        }

        /// <summary>
        /// 拷贝并返回新向量
        /// </summary>
        /// <returns></returns>
        public Vector Copy()
        {
            return new Vector(_data);
        }

        /// <summary>
        /// 拷贝到目标数组des，并指定在des中的起始索引
        /// </summary>
        /// <param name="des"></param>
        /// <param name="desDex"></param>
        public void CopyTo(Array des, int desDex = 0)
        {
            Buffer.BlockCopy(_data, 0, des, desDex * sizeof(double),
                Length * sizeof(double));
        }

        /// <summary>
        /// 拷贝到目标向量des，并指定在des中的起始索引
        /// </summary>
        /// <param name="des"></param>
        /// <param name="desDex"></param>
        public void CopyTo(Vector des, int desDex = 0)
        {
            Array.Copy(_data, 0, des._data, desDex, _data.Length);
        }

        public override string ToString()
        {
            return "[" + string.Join(", ", _data) + "]";
        }
        #endregion
    }
    
    /// <summary>
    /// 矩阵
    /// </summary>
    public class Matrix
    {
        #region 保护成员
        /// <summary>
        /// 存储矩阵数据的二维数组
        /// </summary>
        protected double[,] _data;
        #endregion

        #region 公有属性
        /// <summary>
        /// 矩阵行数
        /// </summary>
        public int RowCount => _data.GetLength(0);

        /// <summary>
        /// 矩阵列数
        /// </summary>
        public int ColumnCount => _data.GetLength(1);
        #endregion

        #region 构造方法
        /// <summary>
        /// 构造一个空矩阵
        /// </summary>
        public Matrix()
        {
            _data = new double[0, 0];
        }

        /// <summary>
        /// 构造rowCount行的方阵，默认元素均为0；
        /// fillOne为true时，元素均为1
        /// </summary>
        /// <param name="rowCount"></param>
        /// <param name="fillOne"></param>
        public Matrix(int rowCount, bool fillOne = false)
            : this(rowCount, rowCount, fillOne)
        {

        }

        /// <summary>
        /// 构造rowCount行、colCount列的矩阵，默认元素均为0；
        /// fillOne为true时，元素均为1
        /// </summary>
        /// <param name="rowCount"></param>
        /// <param name="colCount"></param>
        /// <param name="fillOne"></param>
        public Matrix(int rowCount, int colCount, bool fillOne = false)
        {
            _data = new double[rowCount, colCount];
            if (fillOne)
            {
                for (int i = 0; i < rowCount; i++)
                {
                    for (int j = 0; j < colCount; j++)
                    {
                        _data[i, j] = 1;
                    }
                }
            }
        }

        /// <summary>
        /// 根据二维数组构造矩阵
        /// </summary>
        /// <param name="data"></param>
        public Matrix(double[,] data)
        {
            _data = new double[data.GetLength(0), data.GetLength(1)];
            Array.Copy(data, _data, data.Length);
        }

        /// <summary>
        /// 根据一系列行向量构造新矩阵，这些向量具有相同的长度；
        /// 若要作为列向量构造，请对得到的矩阵进行转置
        /// </summary>
        /// <param name="rows"></param>
        /// <exception cref="Exception"></exception>
        public Matrix(params Vector[] rows)
        {
            int rowCount = rows.Length;
            if (rowCount == 0)
            {
                _data = new double[0, 0];
                return;
            }
            int colCount = rows[0].Length;
            for (int i = 1; i < rowCount; i++)
            {
                if (rows[i].Length != colCount)
                    throw new Exception("各行向量的长度不同，无法组成矩阵！");
            }
            _data = new double[rowCount, colCount];
            for (int i = 0; i < rowCount; i++)
            {
                rows[i].CopyTo(_data, i * colCount);
            }
        }
        #endregion

        #region 公共方法
        /// <summary>
        /// 返回size行size列的单位矩阵
        /// </summary>
        /// <param name="size"></param>
        /// <returns></returns>
        public static Matrix Identity(int size)
        {
            Matrix m = new Matrix(size, size);
            for (int i = 0; i < size; i++)
            {
                m[i, i] = 1;
            }
            return m;
        }

        /// <summary>
        /// 返回size行size列的Hilbert矩阵
        /// </summary>
        /// <param name="size"></param>
        /// <returns></returns>
        public static Matrix Hilbert(int size)
        {
            Matrix m = new Matrix(size);
            for (int i = 0; i < size; i++)
            {
                for (int j = 0; j < size; j++)
                {
                    m[i, j] = 1.0 / (i + j + 1);
                }
            }
            return m;
        }

        /// <summary>
        /// 根据对角线元素构造对角方阵
        /// </summary>
        /// <param name="diag"></param>
        /// <returns></returns>
        public static Matrix Diagonal(Vector diag)
        {
            Matrix m = new Matrix(diag.Length, diag.Length);
            for (int i = 0; i < diag.Length; i++)
            {
                m[i, i] = diag[i];
            }
            return m;
        }

        /// <summary>
        /// 获取矩阵中第rowDex行，第colDex列的元素
        /// </summary>
        /// <param name="rowDex"></param>
        /// <param name="colDex"></param>
        /// <returns></returns>
        public double this[int rowDex, int colDex]
        {
            get => _data[rowDex, colDex];
            set => _data[rowDex, colDex] = value;
        }

        /// <summary>
        /// 获取矩阵中的第rowDex行，作为向量返回；
        /// 对向量元素的修改不会反映到矩阵中
        /// </summary>
        /// <param name="rowDex"></param>
        /// <returns></returns>
        public Vector GetRow(int rowDex)
        {
            double[] data = new double[ColumnCount];
            Buffer.BlockCopy(_data, rowDex * ColumnCount * sizeof(double),
                data, 0, ColumnCount * sizeof(double));
            return new Vector(data);
        }

        /// <summary>
        /// 设置矩阵的第rowDex行，其中row的长度与矩阵列数相同
        /// </summary>
        /// <param name="rowDex"></param>
        /// <param name="row"></param>
        /// <exception cref="Exception"></exception>
        public void SetRow(int rowDex, Vector row)
        {
            if (row.Length != ColumnCount)
                throw new Exception("行向量长度与矩阵列数不同，不能设置！");
            row.CopyTo(_data, rowDex * ColumnCount);
        }

        /// <summary>
        /// 获取矩阵中的第colDex列，作为向量返回；
        /// 对向量元素的修改不会反映到矩阵中
        /// </summary>
        /// <param name="colDex"></param>
        /// <returns></returns>
        public Vector GetColumn(int colDex)
        {
            Vector v = new Vector(RowCount);
            for (int i = 0; i < RowCount; i++)
            {
                v[i] = this[i, colDex];
            }
            return v;
        }

        /// <summary>
        /// 设置矩阵的第colDex列，其中col的长度与矩阵行数相同
        /// </summary>
        /// <param name="colDex"></param>
        /// <param name="col"></param>
        /// <exception cref="Exception"></exception>
        public void SetColumn(int colDex, Vector col)
        {
            if (col.Length != RowCount)
                throw new Exception("列向量长度与矩阵行数不同，不能设置！");
            for (int i = 0; i < RowCount; i++)
            {
                this[i, colDex] = col[i];
            }
        }

        /// <summary>
        /// 获取矩阵对角线元素组成的向量；
        /// 返回向量的长度为行数与列数的较小值
        /// </summary>
        /// <returns></returns>
        public Vector GetDiagonal()
        {
            Vector v = new Vector(Math.Min(RowCount, ColumnCount));
            for (int i = 0; i < v.Length; i++)
            {
                v[i] = this[i, i];
            }
            return v;
        }

        /// <summary>
        /// 正号，返回m1的拷贝
        /// </summary>
        /// <param name="m1"></param>
        /// <returns></returns>
        public static Matrix operator +(Matrix m1)
        {
            return m1.Copy();
        }

        /// <summary>
        /// 负号，返回m1的相反矩阵
        /// </summary>
        /// <param name="m1"></param>
        /// <returns></returns>
        public static Matrix operator -(Matrix m1)
        {
            Matrix m = new Matrix(m1.RowCount, m1.ColumnCount);
            for (int i = 0; i < m1.RowCount; i++)
            {
                for (int j = 0; j < m1.ColumnCount; j++)
                {
                    m[i, j] = -m1[i, j];
                }
            }
            return m;
        }

        /// <summary>
        /// 矩阵加法
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="m2"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Matrix operator +(Matrix m1, Matrix m2)
        {
            if (m1.RowCount != m2.RowCount || m1.ColumnCount != m2.ColumnCount)
                throw new Exception("两矩阵形状不同，不能相加！");
            Matrix m = new Matrix(m1.RowCount, m1.ColumnCount);
            for (int i = 0; i < m1.RowCount; i++)
            {
                for (int j = 0; j < m1.ColumnCount; j++)
                {
                    m[i, j] = m1[i, j] + m2[i, j];
                }
            }
            return m;
        }

        /// <summary>
        /// 矩阵减法
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="m2"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Matrix operator -(Matrix m1, Matrix m2)
        {
            if (m1.RowCount != m2.RowCount || m1.ColumnCount != m2.ColumnCount)
                throw new Exception("两矩阵形状不同，不能相减！");
            Matrix m = new Matrix(m1.RowCount, m1.ColumnCount);
            for (int i = 0; i < m1.RowCount; i++)
            {
                for (int j = 0; j < m1.ColumnCount; j++)
                {
                    m[i, j] = m1[i, j] - m2[i, j];
                }
            }
            return m;
        }

        /// <summary>
        /// 矩阵乘法
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="m2"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Matrix operator *(Matrix m1, Matrix m2)
        {
            if (m1.ColumnCount != m2.RowCount)
                throw new Exception("左矩阵列数与右矩阵行数不同，不能相乘！");
            Matrix m = new Matrix(m1.RowCount, m2.ColumnCount);
            for (int i = 0; i < m.RowCount; i++)
            {
                for (int j = 0; j < m.ColumnCount; j++)
                {
                    for (int k = 0; k < m1.ColumnCount; k++)
                    {
                        m[i, j] += m1[i, k] * m2[k, j];
                    }
                }
            }
            return m;
        }

        /// <summary>
        /// 数乘矩阵
        /// </summary>
        /// <param name="scale"></param>
        /// <param name="m1"></param>
        /// <returns></returns>
        public static Matrix operator *(double scale, Matrix m1)
        {
            Matrix m = new Matrix(m1.RowCount, m1.ColumnCount);
            for (int i = 0; i < m1.RowCount; i++)
            {
                for (int j = 0; j < m1.ColumnCount; j++)
                {
                    m[i, j] = m1[i, j] * scale;
                }
            }
            return m;
        }

        /// <summary>
        /// 数乘矩阵
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="scale"></param>
        /// <returns></returns>
        public static Matrix operator *(Matrix m1, double scale)
        {
            return scale * m1;
        }

        /// <summary>
        /// 矩阵除数
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="scale"></param>
        /// <returns></returns>
        public static Matrix operator /(Matrix m1, double scale)
        {
            return 1 / scale * m1;
        }

        /// <summary>
        /// 矩阵乘以(列)向量，返回(列)向量
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="v1"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Vector operator *(Matrix m1, Vector v1)
        {
            if (m1.ColumnCount != v1.Length)
                throw new Exception("矩阵列数与列向量长度不同，不能进行矩阵左乘！");
            Vector v = new Vector(m1.RowCount);
            for (int i = 0; i < v.Length; i++)
            {
                for (int j = 0; j < v1.Length; j++)
                {
                    v[i] += m1[i, j] * v1[j];
                }
            }
            return v;
        }

        /// <summary>
        /// (行)向量乘以矩阵，返回(行)向量
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="m1"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Vector operator *(Vector v1, Matrix m1)
        {
            if (v1.Length != m1.RowCount)
                throw new Exception("行向量长度与矩阵行数不同，不能进行矩阵右乘！");
            Vector v = new Vector(m1.ColumnCount);
            for (int i = 0; i < v.Length; i++)
            {
                for (int j = 0; j < v1.Length; j++)
                {
                    v[i] += m1[j, i] * v1[j];
                }
            }
            return v;
        }

        /// <summary>
        /// 按元素相乘
        /// </summary>
        /// <param name="m1"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public Matrix Multiply(Matrix m1)
        {
            if (RowCount != m1.RowCount || ColumnCount != m1.ColumnCount)
                throw new Exception("两矩阵形状不同，不能按元素相乘！");
            Matrix m = new Matrix(RowCount, ColumnCount);
            for (int i = 0; i < RowCount; i++)
            {
                for (int j = 0; j < ColumnCount; j++)
                {
                    m[i, j] = this[i, j] * m1[i, j];
                }
            }
            return m;
        }

        /// <summary>
        /// 按元素相除
        /// </summary>
        /// <param name="m1"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public Matrix Divide(Matrix m1)
        {
            if (RowCount != m1.RowCount || ColumnCount != m1.ColumnCount)
                throw new Exception("两矩阵形状不同，不能按元素相除！");
            Matrix m = new Matrix(RowCount, ColumnCount);
            for (int i = 0; i < RowCount; i++)
            {
                for (int j = 0; j < ColumnCount; j++)
                {
                    m[i, j] = this[i, j] / m1[i, j];
                }
            }
            return m;
        }

        /// <summary>
        /// 返回当前矩阵的转置矩阵
        /// </summary>
        /// <returns></returns>
        public Matrix Transpose()
        {
            Matrix m = new Matrix(ColumnCount, RowCount);
            for (int i = 0; i < RowCount; i++)
            {
                for (int j = 0; j < ColumnCount; j++)
                {
                    m[j, i] = this[i, j];
                }
            }
            return m;
        }

        /// <summary>
        /// 返回矩阵A的行列式值
        /// </summary>
        /// <returns></returns>
        public double Determinant()
        {
            return GaussElimination.Determinant(this);
        }

        /// <summary>
        /// 返回A的逆矩阵；
        /// 当A奇异时，返回null
        /// </summary>
        /// <returns></returns>
        public Matrix Inverse()
        {
            return GaussElimination.Inverse(this);
        }

        /// <summary>
        /// 判断是否与m1的所有元素均相同
        /// </summary>
        /// <param name="m1"></param>
        /// <returns></returns>
        public bool Equals(Matrix m1)
        {
            if (RowCount != m1.RowCount || ColumnCount != m1.ColumnCount)
                return false;
            for (int i = 0; i < RowCount; i++)
            {
                for (int j = 0; j < ColumnCount; j++)
                {
                    if (this[i, j] != m1[i, j])
                        return false;
                }
            }
            return true;
        }

        /// <summary>
        /// 返回子矩阵，包含原矩阵的begin到end-1行
        /// </summary>
        /// <param name="begin"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        public Matrix SubRows(int begin, int end)
        {
            Matrix m = new Matrix(end - begin, ColumnCount);
            for (int i = begin; i < end; i++)
            {
                m.SetRow(i - begin, GetRow(i));
            }
            return m;
        }

        /// <summary>
        /// 返回子矩阵，包含原矩阵的begin到end-1列
        /// </summary>
        /// <param name="begin"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        public Matrix SubColumns(int begin, int end)
        {
            var m = new Matrix(RowCount, end - begin);
            for (int i = 0; i < RowCount; i++)
            {
                Array.Copy(_data, i * ColumnCount + begin, m._data,
                    i * m.ColumnCount, m.ColumnCount);
            }
            return m;
        }

        /// <summary>
        /// 返回当前矩阵与m1拼接后的新矩阵，默认横向拼接(添加列数)
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="addNewRow"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public Matrix Concat(Matrix m1, bool addNewRow = false)
        {
            if (addNewRow)
            {
                if (ColumnCount != m1.ColumnCount)
                    throw new Exception("矩阵列数不同，不能竖向拼接！");
                Matrix m = new Matrix(RowCount + m1.RowCount, ColumnCount);
                Array.Copy(_data, m._data, _data.Length);
                Array.Copy(m1._data, 0, m._data, _data.Length, m1._data.Length);
                return m;
            }
            else
            {
                if (RowCount != m1.RowCount)
                    throw new Exception("矩阵行数不同，不能横向拼接！");
                Matrix m = new Matrix(RowCount, ColumnCount + m1.ColumnCount);
                for (int i = 0; i < RowCount; i++)
                {
                    Array.Copy(_data, i * ColumnCount, m._data,
                        i * m.ColumnCount, ColumnCount);
                    Array.Copy(m1._data, i * m1.ColumnCount, m._data,
                        i * m.ColumnCount + ColumnCount, m1.ColumnCount);
                }
                return m;
            }
        }

        /// <summary>
        /// 矩阵的拷贝
        /// </summary>
        /// <returns></returns>
        public Matrix Copy()
        {
            return new Matrix(_data);
        }

        public override string ToString()
        {
            object[] rows = new Vector[RowCount];
            for (int i = 0; i < RowCount; i++)
            {
                rows[i] = GetRow(i);
            }
            return "[" + string.Join(",\n", rows) + "]";
        }
        #endregion
    }

    /// <summary>
    /// 高斯消去法
    /// </summary>
    public class GaussElimination
    {
        /// <summary>
        /// 返回矩阵A经过列主元高斯消去后形成的上三角阵；
        /// 如果矩阵A前RowCount列组成的矩阵奇异，返回null
        /// </summary>
        /// <param name="A"></param>
        /// <returns></returns>
        public static Matrix ToUpper(Matrix A)
        {
            return ToUpper(A, out int _);
        }

        /// <summary>
        /// 返回矩阵A经过列主元高斯消去后形成的上三角阵；
        /// swapCount将被赋值为列主元消去时交换两行的次数；
        /// 如果矩阵A前RowCount列组成的矩阵奇异，返回null
        /// </summary>
        /// <param name="A"></param>
        /// <returns></returns>
        public static Matrix ToUpper(Matrix A, out int swapCount)
        {
            Matrix U = A.Copy();

            swapCount = 0;
            for (int i = 0; i < U.RowCount - 1; i++)
            {
                int pivot = GetColumnPivot(U, i, i);
                if (pivot == -1)
                    return null;

                if (pivot != i)
                {
                    SwapRow(U, i, pivot);
                    swapCount++;
                }

                var rowVec = U.GetRow(i);
                for (int j = i + 1; j < U.RowCount; j++)
                {
                    double factor = U[j, i] / U[i, i];
                    U.SetRow(j, U.GetRow(j) - factor * rowVec);
                }
            }
            return U;
        }

        /// <summary>
        /// 返回矩阵colDex列上从beginRowDex开始的绝对值最大元素所在行；
        /// 如果查找的所有元素均为0，返回-1
        /// </summary>
        /// <param name="m"></param>
        /// <param name="colDex"></param>
        /// <param name="beginRowDex"></param>
        /// <returns></returns>
        public static int GetColumnPivot(Matrix m, int colDex, int beginRowDex)
        {
            int pivot = -1;
            double max = 0;
            for (int i = beginRowDex; i < m.RowCount; i++)
            {
                if (Math.Abs(m[i, colDex]) > max)
                {
                    pivot = i;
                    max = Math.Abs(m[i, colDex]);
                }
            }
            return pivot;
        }

        /// <summary>
        /// 交换矩阵的两行，会改变原矩阵
        /// </summary>
        /// <param name="m"></param>
        /// <param name="rowDex1"></param>
        /// <param name="rowDex2"></param>
        public static void SwapRow(Matrix m, int rowDex1, int rowDex2)
        {
            if (rowDex1 == rowDex2)
                return;
            var row1 = m.GetRow(rowDex1);
            m.SetRow(rowDex1, m.GetRow(rowDex2));
            m.SetRow(rowDex2, row1);
        }

        /// <summary>
        /// 返回矩阵A的行列式值
        /// </summary>
        /// <param name="A"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static double Determinant(Matrix A)
        {
            if (A.RowCount != A.ColumnCount)
                throw new Exception("A不是方阵，无法求行列式值！");

            int swapCount;
            Matrix U = ToUpper(A, out swapCount);

            if (U is null)
                return 0;

            double product = 1;
            for (int i = 0; i < A.RowCount; i++)
            {
                product *= U[i, i];
            }

            return swapCount % 2 == 0 ? product : -product;
        }

        /// <summary>
        /// 返回A的逆矩阵；
        /// 当A奇异时，返回null
        /// </summary>
        /// <param name="A"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Matrix Inverse(Matrix A)
        {
            if (A.RowCount != A.ColumnCount)
                throw new Exception("A不是方阵，无法求逆矩阵！");

            var AI = A.Concat(Matrix.Identity(A.RowCount));

            AI = ToUpper(AI);
            if (AI is null)
                return null;

            for (int i = AI.RowCount - 1; i >= 0; i--)
            {
                var rowVec = AI.GetRow(i);
                rowVec /= rowVec[i];
                AI.SetRow(i, rowVec);
                for (int j = 0; j < i; j++)
                {
                    AI.SetRow(j, AI.GetRow(j) - AI[j, i] * rowVec);
                }
            }

            return AI.SubColumns(A.ColumnCount, AI.ColumnCount);
        }

        /// <summary>
        /// 列主元高斯消元法求解Ax=b；
        /// 如果矩阵A奇异，则返回null
        /// </summary>
        /// <param name="A"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Vector Solve(Matrix A, Vector b)
        {
            if (A.RowCount != A.ColumnCount)
                throw new Exception("A不是方阵，无法求解Ax=b！");
            if (A.RowCount != b.Length)
                throw new Exception("A的行数与b的元素个数不同，无法求解Ax=b！");

            var Ab = A.Concat(new Matrix(b).Transpose());
            Ab = ToUpper(Ab);

            if (Ab is null)
                return null;

            var U = Ab.SubColumns(0, A.ColumnCount);
            var Ub = Ab.GetColumn(A.ColumnCount);

            var x = new Vector(A.RowCount);
            for (int i = x.Length - 1; i >= 0; i--)
            {
                x[i] = (Ub[i] - U.GetRow(i) * x) / U[i, i];
            }
            return x;
        }
    }

    public class LeastSquare
    {
        
        /// <summary>
        /// 线性最小二乘拟合，用一组基向量[Y1,Y2,……,Yn]的线性组合拟合目标向量Y；
        /// 输出各向量的系数[a1,a2,……,an]，使Y*=a1*Y1+a2*Y2+……+an*Yn满足||Y*-Y||2最小；
        /// 当基向量线性相关时，抛出异常；
        /// 例如，欲求Y*=a1*exp(X)+a2*X，可令基向量[Y1,Y2]=[exp(X),X]
        /// </summary>
        /// <param name="targetY"></param>
        /// <param name="baseYs"></param>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        public static Vector LinearFit(Vector targetY, params Vector[] baseYs)
        {
            for (int i = 0; i < baseYs.Length; i++)
            {
                if (baseYs[i].Length != targetY.Length)
                    throw new Exception("存在基向量的长度与目标向量的元素个数不同，无法拟合！");
            }

            Matrix A = new Matrix(baseYs.Length);
            Vector b = new Vector(baseYs.Length);

            for (int i = 0; i < baseYs.Length; i++)
            {
                for (int j = 0; j < i; j++)
                {
                    A[i, j] = baseYs[i] * baseYs[j];
                    A[j, i] = A[i, j];
                }
                A[i, i] = baseYs[i] * baseYs[i];

                b[i] = baseYs[i] * targetY;
            }

            Vector ans = GaussElimination.Solve(A, b);
            if (ans is null)
                throw new Exception("基向量线性相关，无法进行线性最小二乘拟合！");

            {
                Vector predictY = new Vector(targetY.Length);
                for (int i = 0; i < baseYs.Length; i++)
                {
                    predictY += ans[i] * baseYs[i];
                }
                Console.WriteLine("拟合误差：" + Norm.Two(predictY - targetY));
            }

            return ans;
        }

        
    }

}
